# Import the necessary libraries
import serial
import os
import sys
import time
import rospy
import collections
from sensor_msgs.msg import Imu
from std_msgs.msg import Header
from std_msgs.msg import Int16
import argparse

# Define a function to convert a four-digit hexadecimal number to a signed integer
def int16_t(num):
    # Check if the integer is greater than or equal to 2^15
    if num >= 2**15:
        # Convert the integer to a signed integer
        signed_int = num - 2**16
    else:
        # The integer is already a signed integer
        signed_int = num
    return signed_int


parser = argparse.ArgumentParser()
parser.add_argument('--tty_usb',type=str,default='/dev/ttyUSB0')
args = parser.parse_args()
print("usb ",args.tty_usb)
ser = serial.Serial(args.tty_usb, 921600)


rospy.init_node('encoder_node', anonymous=True)

global ctrl_command
ctrl_command = "AAAF051102013AAC"

      
pub = rospy.Publisher('encoder_data', Header, queue_size=1)
dq = collections.deque(maxlen=4)

while not rospy.is_shutdown():

    # high_byte = ser.read(1)[0]
    # low_byte = ser.read(1)[0]
    # encoder_data = high_byte*256 + low_byte
    # if int(encoder_data)>=0 and int(encoder_data)<=4096:
    #     print("encoder ", int(encoder_data))
    # header = Header()
    # header.stamp = rospy.Time.now()
    # header.seq = encoder_data
    # header.frame_id = ctrl_command
    # pub.publish(header)



    print("------RUNNING-------")
    # ee = ser.read(8)
    # print("ee ",ee)


    encoder_data = 0
    data = ser.read(1)[0]
    # if data == 170:
    #     data = ser.read(1)[0]
    #     if data == 175:
    #         data = ser.read(1)[0]

    if data == 0:
        data = ser.read(1)[0]
        if data == 218:
            data = ser.read(1)[0]
            if data == 2:
                high_byte = ser.read(1)[0]
                low_byte = ser.read(1)[0]
                encoder_data = high_byte*256 + low_byte
                if int(encoder_data)>=0 and int(encoder_data)<=4096:
                    print("encoder ", int(encoder_data))
                    header = Header()
                    header.stamp = rospy.Time.now()
                    header.seq = encoder_data
                    header.frame_id = ctrl_command
                    pub.publish(header)
                    data = ser.read(3)[0]

    else:
        continue
    




